﻿using System;

using OpenTK.Graphics.OpenGL;
using OpenTK;

namespace MosCraft.Utilities
{
    public class ViewFrustum
    {
        internal float[] ProjectionMatrix = new float[16];
        internal float[] ModelviewMatrix = new float[16];
        internal float[] clip = new float[16];
        internal Vector4[] frustum = new Vector4[6];

        public ViewFrustum()
        {
            for (int i = 0; i < 6; i++)
                frustum[i] = new Vector4();
        }

        public void updateFrustum()
        {
            GL.GetFloat(GetPName.ProjectionMatrix, ProjectionMatrix);
            GL.GetFloat(GetPName.ModelviewMatrix, ModelviewMatrix);

            #region clips
            clip[0] = ModelviewMatrix[0] * ProjectionMatrix[0] + ModelviewMatrix[1] * ProjectionMatrix[4] + ModelviewMatrix[2] * ProjectionMatrix[8] + ModelviewMatrix[3] * ProjectionMatrix[12];
            clip[1] = ModelviewMatrix[0] * ProjectionMatrix[1] + ModelviewMatrix[1] * ProjectionMatrix[5] + ModelviewMatrix[2] * ProjectionMatrix[9] + ModelviewMatrix[3] * ProjectionMatrix[13];
            clip[2] = ModelviewMatrix[0] * ProjectionMatrix[2] + ModelviewMatrix[1] * ProjectionMatrix[6] + ModelviewMatrix[2] * ProjectionMatrix[10] + ModelviewMatrix[3] * ProjectionMatrix[14];
            clip[3] = ModelviewMatrix[0] * ProjectionMatrix[3] + ModelviewMatrix[1] * ProjectionMatrix[7] + ModelviewMatrix[2] * ProjectionMatrix[11] + ModelviewMatrix[3] * ProjectionMatrix[15];

            clip[4] = ModelviewMatrix[4] * ProjectionMatrix[0] + ModelviewMatrix[5] * ProjectionMatrix[4] + ModelviewMatrix[6] * ProjectionMatrix[8] + ModelviewMatrix[7] * ProjectionMatrix[12];
            clip[5] = ModelviewMatrix[4] * ProjectionMatrix[1] + ModelviewMatrix[5] * ProjectionMatrix[5] + ModelviewMatrix[6] * ProjectionMatrix[9] + ModelviewMatrix[7] * ProjectionMatrix[13];
            clip[6] = ModelviewMatrix[4] * ProjectionMatrix[2] + ModelviewMatrix[5] * ProjectionMatrix[6] + ModelviewMatrix[6] * ProjectionMatrix[10] + ModelviewMatrix[7] * ProjectionMatrix[14];
            clip[7] = ModelviewMatrix[4] * ProjectionMatrix[3] + ModelviewMatrix[5] * ProjectionMatrix[7] + ModelviewMatrix[6] * ProjectionMatrix[11] + ModelviewMatrix[7] * ProjectionMatrix[15];

            clip[8] = ModelviewMatrix[8] * ProjectionMatrix[0] + ModelviewMatrix[9] * ProjectionMatrix[4] + ModelviewMatrix[10] * ProjectionMatrix[8] + ModelviewMatrix[11] * ProjectionMatrix[12];
            clip[9] = ModelviewMatrix[8] * ProjectionMatrix[1] + ModelviewMatrix[9] * ProjectionMatrix[5] + ModelviewMatrix[10] * ProjectionMatrix[9] + ModelviewMatrix[11] * ProjectionMatrix[13];
            clip[10] = ModelviewMatrix[8] * ProjectionMatrix[2] + ModelviewMatrix[9] * ProjectionMatrix[6] + ModelviewMatrix[10] * ProjectionMatrix[10] + ModelviewMatrix[11] * ProjectionMatrix[14];
            clip[11] = ModelviewMatrix[8] * ProjectionMatrix[3] + ModelviewMatrix[9] * ProjectionMatrix[7] + ModelviewMatrix[10] * ProjectionMatrix[11] + ModelviewMatrix[11] * ProjectionMatrix[15];

            clip[12] = ModelviewMatrix[12] * ProjectionMatrix[0] + ModelviewMatrix[13] * ProjectionMatrix[4] + ModelviewMatrix[14] * ProjectionMatrix[8] + ModelviewMatrix[15] * ProjectionMatrix[12];
            clip[13] = ModelviewMatrix[12] * ProjectionMatrix[1] + ModelviewMatrix[13] * ProjectionMatrix[5] + ModelviewMatrix[14] * ProjectionMatrix[9] + ModelviewMatrix[15] * ProjectionMatrix[13];
            clip[14] = ModelviewMatrix[12] * ProjectionMatrix[2] + ModelviewMatrix[13] * ProjectionMatrix[6] + ModelviewMatrix[14] * ProjectionMatrix[10] + ModelviewMatrix[15] * ProjectionMatrix[14];
            clip[15] = ModelviewMatrix[12] * ProjectionMatrix[3] + ModelviewMatrix[13] * ProjectionMatrix[7] + ModelviewMatrix[14] * ProjectionMatrix[11] + ModelviewMatrix[15] * ProjectionMatrix[15];
            #endregion

            #region planes
            // RIGHT
            frustum[0].X = clip[3] - clip[0];
            frustum[0].Y = clip[7] - clip[4];
            frustum[0].Z = clip[11] - clip[8];
            frustum[0].W = clip[15] - clip[12];
            frustum[0].Normalize();

            // LEFT
            frustum[1].X = clip[3] + clip[0];
            frustum[1].Y = clip[7] + clip[4];
            frustum[1].Z = clip[11] + clip[8];
            frustum[1].W = clip[15] + clip[12];
            frustum[1].Normalize();

            // BOTTOM
            frustum[2].X = clip[3] + clip[1];
            frustum[2].Y = clip[7] + clip[5];
            frustum[2].Z = clip[11] + clip[9];
            frustum[2].W = clip[15] + clip[13];
            frustum[2].Normalize();

            // TOP
            frustum[3].X = clip[3] - clip[1];
            frustum[3].Y = clip[7] - clip[5];
            frustum[3].Z = clip[11] - clip[9];
            frustum[3].W = clip[15] - clip[13];
            frustum[3].Normalize();

            // FAR
            frustum[4].X = clip[3] - clip[2];
            frustum[4].Y = clip[7] - clip[6];
            frustum[4].Z = clip[11] - clip[10];
            frustum[4].W = clip[15] - clip[14];
            frustum[4].Normalize();

            // NEAR
            frustum[5].X = clip[3] + clip[2];
            frustum[5].Y = clip[7] + clip[6];
            frustum[5].Z = clip[11] + clip[10];
            frustum[5].W = clip[15] + clip[14];
            frustum[5].Normalize();
            #endregion

        }

        public bool intersects(double x, double y, double z)
        {
            for (int i = 0; i < 6; i++)
            {
                if (frustum[i].X * x + frustum[i].Y * y + frustum[i].Z * z + frustum[i].W <= 0)
                {
                    return false;
                }
            }
            return true;
        }

        public bool isBoxInFrustum(Vector3d min, Vector3d max)
        {
            return this.isBoxInFrustum(min.X, min.Y, min.Z, max.X, max.Y, max.Z);
        }

        public bool isBoxInFrustum(double minX, double minY, double minZ, double maxX, double maxY, double maxZ)
        {
            for (int i = 0; i < 6; i++)
            {
                if ((double)frustum[i].X * minX + (double)frustum[i].Y * minY + (double)frustum[i].Z * minZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * maxX + (double)frustum[i].Y * minY + (double)frustum[i].Z * minZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * minX + (double)frustum[i].Y * maxY + (double)frustum[i].Z * minZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * maxX + (double)frustum[i].Y * maxY + (double)frustum[i].Z * minZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * minX + (double)frustum[i].Y * minY + (double)frustum[i].Z * maxZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * maxX + (double)frustum[i].Y * minY + (double)frustum[i].Z * maxZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * minX + (double)frustum[i].Y * maxY + (double)frustum[i].Z * maxZ + (double)frustum[i].W <= 0.0D
                    && (double)frustum[i].X * maxX + (double)frustum[i].Y * maxY + (double)frustum[i].Z * maxZ + (double)frustum[i].W <= 0.0D)
                {
                    return false;
                }
            }

            return true;
        }

        public bool isBoundingBoxInFrustum(AxisAlignedBB axisalignedbb)
        {
            return this.isBoxInFrustum(axisalignedbb.min.X, axisalignedbb.min.Y, axisalignedbb.min.Z, axisalignedbb.max.X, axisalignedbb.max.Y, axisalignedbb.max.Z);
        }

    }
}
